//
// Created by HP on 2024/2/6.
//

#include "aurora_foc.h"
#include "math.h"

#include "rtthread.h"

Aurora_FOC::Aurora_FOC(){

    Pole_Pair = 7;
    Ts = 1;
    Udc = 12;
    Ud = 0.0;
    Uq = 1.5f;

    mechanical_offset = 9.11f; //21.25f
    ADC_Uu_offset = 2048;
    ADC_Uv_offset = 2048;
    ADC_Uw_offset = 2048;

    target_Iq = 0.0f;
    target_Id = 0.0f;

    motor_rotate_direct = DISABLE;
    //对PID参数进行初始化
    I_pid_q.Init();
    I_pid_d.Init();
    Speed_pid.Init();

    I_pid_q.set_PID(3.5,0.05,0);
    I_pid_q.set_Iintegral_Limit(200.0f);

    I_pid_d.set_PID(3,0.03,0);
    I_pid_d.set_Iintegral_Limit(200.0f);

    Speed_pid.set_PID(-0.145f,-0.005f,0);
    Speed_pid.set_Iintegral_Limit(200.0f);
    Speed_pid.set_desireValue(0.08);

    Position_pid.set_PID(5.0,0.020,0);
    Position_pid.set_Iintegral_Limit(200.0f);
    Position_pid.set_desireValue(0);

    _wave_type = SEND_NONE;

    pos_gap = 0;
    position = 0;

    _tx_max = 1;
    _tx_min = 0;
    _Ix_max = 0.22f;
    _Ix_min = -0.22f;
    _Iqd_max= 0.3f;
    _Iqd_min= -0.3f;
    _Ialphabeta_max = 0.22f;
    _Ialphabeta_min = -0.22f;

    UqUd_Limit_Max = 6.0f;
    IqId_Limit_Max = 3.0f;
}




/** 计算角度的 sin 和 cos 值**/
void Aurora_FOC::cal_angle_sincos(){
    float electrical_angle = mechanical_angle * Pole_Pair ;//*  0.0174532922222f; //将机械角度转化为电角度(弧度)
    while (electrical_angle>360) electrical_angle-= 360.0f;
    electrical_angle *= 0.0174532922222f;
    cosVal = cos(electrical_angle);
    sinVal = sin(electrical_angle);
}

/**clark变换: 将三相电流 变换为 两相**/
void Aurora_FOC::clark(){
    Ialpha = Ia;
    Ibeta =  0.5773502691896f * (Ia +  2 * Ib);  //1/ sqrtf(3) = 0.5773502691896

    if(_wave_type == SEND_FOC_IalphaIbeta){
        sendWave(_Ialphabeta_max,_Ialphabeta_min,Ialpha,0,Ibeta,4.4f);
    }

}
/**反clark变换: 将两相电压 变换为 三相 **/
void Aurora_FOC::iclark(){
    Ua = Ualpha;
    Ub = -Ualpha/2 + sqrtf(3)/2 * Ubeta;
    Uc = -Ualpha/2 - sqrtf(3)/2 * Ubeta;
}
/**park变换: 将两相静止电流 变换为 两相动态电流**/

void Aurora_FOC::park(){
    Iq =  Iq * 0.3f + 0.7f *(-Ialpha * sinVal + Ibeta * cosVal);     //实际的Iq
    Id =  Id * 0.3f + 0.7f *(Ialpha * cosVal + Ibeta * sinVal);      //实际的Id


    if(_wave_type == SEND_FOC_IqId){
        sendWave(_Iqd_max,_Iqd_min,Iq,0,Id,3.3f);
    }

}
/** park反变换: 将两相动态电压 变换为 两相 静止 电压
 * angle :  输入电角度
**/
void Aurora_FOC::ipark(){
    Ualpha = Ud * cosVal - Uq * sinVal;
    Ubeta  = Uq * cosVal + Ud * sinVal;
}


/**生成svpwm波**/
void Aurora_FOC::generate_svpwm(){
    float K = SQRT_3*Ts/Udc;
    float U1,U2,U3;
    float T0=0,T1,T2,T3,T4,T5,T6,T7;
    float Ta,Tb,Tc;
    uint8_t A,B,C,N,sector;
    U1 = Ubeta;
    U2 = -SQRT_3DIV2 * Ualpha - Ubeta/2;
    U3 = SQRT_3DIV2* Ualpha - Ubeta/2;

    A = (U1 > 0) ? 1 : 0;
    B = (U2 > 0) ? 1 : 0;
    C = (U3 > 0) ? 1 : 0;
    N = (A<<2) + (B<<1) + (C);

    //扇区判断
    sector = ( N == 5) ? 1:
             ( N == 4) ? 2:
             ( N == 6) ? 3:
             ( N == 2) ? 4:
             ( N == 3) ? 5: 6;
    //生成矢量作用时间
    if( sector == 1 ){
        T4 = K * U3;
        T6 = K * U1;
        if( Ts < T4 + T6){
            float k = Ts / (T4 + T6);
            T4 = k * T4;
            T6 = k * T6;
        }
        T7 = (Ts - T4 -T6) / 2;

        Ta = T4 + T6 + T0 + T7;
        Tb = T6 + T0 + T7;
        Tc = T0 + T7;
    }
    else if(sector == 2 ){
        T2 = -K * U3;
        T6 = -K * U2;
        if( Ts < T2 + T6){
            float k = Ts / (T2 + T6);
            T2 = k * T2;
            T6 = k * T6;
        }
        T7 = (Ts - T2 -T6) / 2;

        Ta = T6 + T0 + T7;
        Tb = T2 + T6 + T0 + T7;
        Tc = T0 + T7;
    }
    else if( sector == 3 ){
        T2 = K*U1;
        T3 = K*U2;
        if( Ts < T2 + T3){
            float k = Ts / (T2 + T3);
            T2 = k * T2;
            T3 = k * T3;
        }
        T7 = (Ts - T2 -T3) / 2;

        Ta = T0 + T7;
        Tb = T2 + T3 + T0 + T7;
        Tc = T3 + T0 + T7;

    }
    else if( sector == 4){
        T1 = -K*U1;
        T3 = -K*U3;
        if( Ts < T1 + T3){
            float k = Ts / (T1 + T3);
            T1 = k * T1;
            T3 = k * T3;
        }

        T7 = (Ts - T1 -T3) / 2;

        Ta = T0 + T7;
        Tb = T3 + T0 + T7;
        Tc = T1 + T3 + T0 + T7;
    }
    else if(sector == 5){
        T1 = K*U2;
        T5 = K*U3;
        if( Ts < T1 + T5){
            float k = Ts / (T1 + T5);
            T1 = k * T1;
            T5 = k * T5;
        }
        T7 = (Ts - T1 -T5) / 2;

        Ta = T5 + T0 + T7;
        Tb = T0 + T7;
        Tc = T1 + T5 + T0 + T7;
    }
    else{
        T4 = -K*U2;
        T5 = -K*U1;
        if( Ts < T4 + T5){
            float k = Ts / (T4 + T5);
            T4 = k * T4;
            T5 = k * T5;
        }
        T7 = (Ts - T4 -T5) / 2;

        Ta = T4 + T5 + T0 + T7;
        Tb = T0 + T7;
        Tc = T5 + T0 + T7;
    }
    update_pwm(Ta,Tb,Tc);
}






/** 更新三相电流
 * valid_num : 后面有效电压的个数
 * **/
void Aurora_FOC::update_I(uint8_t valid_num,uint32_t Uu,uint32_t Uv,uint32_t Uw){

    float Uu_ = ((float)((int)Uu - (int)ADC_Uu_offset) / 2048.0f) * 3.3f;
    float Uv_ = ((float)((int)Uv - (int)ADC_Uv_offset) / 2048.0f) * 3.3f;
    float Uw_ = ((float)((int)Uw - (int)ADC_Uw_offset) / 2048.0f) * 3.3f;
    //三相采样
    if( valid_num == 3){
        Ia = Uu_ * 2.857142857f;// (ADC_multiple * Sample_R);
        Ib = Uv_ * 2.857142857f;// (ADC_multiple * Sample_R);
        Ic = Uw_ * 2.857142857f;// (ADC_multiple * Sample_R);
    }
    else if(valid_num == 2){  //两相采样
        Ia = Ia * 0.2f + 0.8f * (Uu_ * 2.857142857f);// (ADC_multiple * Sample_R);
        Ib = Ib * 0.2f + 0.8f * (Uv_ * 2.857142857f);// (ADC_multiple * Sample_R);
        Ic = Ic * 0.2f + 0.8f * (-(Ia + Ib));
    }
    else;  // 单电阻采样不考虑

    if(_wave_type == SEND_FOC_IaIbIc){
        sendWave(_Ix_max,_Ix_min,Ia,Ib,Ic,2.2f);
    }
}

//更新三相pwm的时间
void Aurora_FOC::update_pwm(float tu,float tv,float tw){
    //对时间大小进行限制
    tu = (tu > 0.8f) ? 0.8f : (tu < 0.0f) ? 0.0f : tu;
    tv = (tv > 0.8f) ? 0.8f : (tv < 0.0f) ? 0.0f : tv;
    tw = (tw > 0.8f) ? 0.8f : (tw < 0.0f) ? 0.0f : tw;



    htim->Instance->CCR1 = (uint16_t)(tu * ARR_10Khz);
    htim->Instance->CCR2 = (uint16_t)(tv * ARR_10Khz);
    htim->Instance->CCR3 = (uint16_t)(tw * ARR_10Khz);
//    __HAL_TIM_SetCompare(htim,TIM_CHANNEL_U,(uint16_t)(tu * ARR_10Khz));
//    __HAL_TIM_SetCompare(htim,TIM_CHANNEL_V,(uint16_t)(tv * ARR_10Khz));
//    __HAL_TIM_SetCompare(htim,TIM_CHANNEL_W,(uint16_t)(tw * ARR_10Khz));


    if(_wave_type == SEND_FOC_TaTbTc){
        sendWave(_tx_max,_tx_min,tu,tv,tw,1.1f);
    }

}
/** 暂停pwm输出 **/
void Aurora_FOC::halt_pwm(){
    __HAL_TIM_SetCompare(htim,TIM_CHANNEL_U,0);//ARR_10Khz
    __HAL_TIM_SetCompare(htim,TIM_CHANNEL_V,0);//ARR_10Khz
    __HAL_TIM_SetCompare(htim,TIM_CHANNEL_W,0);//ARR_10Khz
}

/** 使能定时器 pwm输出  不使能中断**/
void Aurora_FOC::start_pwm(){
    HAL_TIM_PWM_Start(htim,TIM_CHANNEL_U);
    HAL_TIM_PWM_Start(htim,TIM_CHANNEL_V);
    HAL_TIM_PWM_Start(htim,TIM_CHANNEL_W);

    HAL_TIMEx_PWMN_Start(htim,TIM_CHANNEL_U);
    HAL_TIMEx_PWMN_Start(htim,TIM_CHANNEL_V);
    HAL_TIMEx_PWMN_Start(htim,TIM_CHANNEL_W);


    __HAL_TIM_SET_COMPARE(htim,TIM_CHANNEL_U,0);
    __HAL_TIM_SET_COMPARE(htim,TIM_CHANNEL_V,0);
    __HAL_TIM_SET_COMPARE(htim,TIM_CHANNEL_W,0);

    __HAL_TIM_ENABLE_IT(htim,TIM_IT_UPDATE); //使能定时器中断
}


/** 使能 adc 电流采样 **/
void Aurora_FOC::start_adc_sample(){
    __HAL_ADC_ENABLE_IT(hadc, ADC_IT_JEOC);
    HAL_ADCEx_InjectedStart(hadc);
    HAL_TIM_PWM_Start(htim,TIM_CHANNEL_InJ); //启动定时器1
}

/** 设置采样的 adc **/
void Aurora_FOC::set_adc(ADC_HandleTypeDef *hadcx){
    hadc = hadcx;
}

/** 设置驱动定时器 及其对应的通道 InJ注入ADC通道**/
void Aurora_FOC::set_tim(TIM_HandleTypeDef *htimx,uint32_t channelu,uint32_t channelv,uint32_t channelw,uint32_t channelInJ){
    htim = htimx;
    TIM_CHANNEL_U = channelu;
    TIM_CHANNEL_V = channelv;
    TIM_CHANNEL_W = channelw;
    TIM_CHANNEL_InJ = channelInJ;
}

/** 设置磁编码器 **/
void Aurora_FOC::set_endocer(AS5047_Driver *driver){
    as5047 = driver;
}
/** 设置目标电流值 **/
void Aurora_FOC::set_target_I(float I_q,float I_d){
    target_Iq = I_q;
    target_Id = I_d;

    I_pid_q.set_desireValue(target_Iq);
    I_pid_d.set_desireValue(target_Id);
}
void Aurora_FOC::set_mechanical_offset(float offset){
    mechanical_offset = offset;
}
void Aurora_FOC::set_UqUd(float _Uq,float _Ud){
    Ud = _Ud;
    Uq = _Uq;
}


void Aurora_FOC::set_UqUdIqIdMAX(float _UqUdMax,float _IqIdMax){
    UqUd_Limit_Max = _UqUdMax;
    IqId_Limit_Max = _IqIdMax;
}

/** 计算 零角度 和 电机的极对数**/
void Aurora_FOC::calibration_angle_pole_pair(){
    uint8_t d;
    Pole_Pair = 1;
    set_UqUd(0.0,0.7);
    mechanical_angle = 0;
    cal_angle_sincos();
    ipark();   //反Park变换
    generate_svpwm();
    HAL_Delay(1000);
    float sum = 0;
    for(d = 0 ;d < 10; d++){
        sum += as5047->as5047_read_angle(false);
    }
    float angle_start = sum / 10.0f;//as5047->as5047_read_angle(false);
    mechanical_angle = 90;
    cal_angle_sincos();
    ipark();   //反Park变换
    generate_svpwm();
    HAL_Delay(1000);
    sum = 0;
    for(d = 0 ;d < 10; d++){
        sum += as5047->as5047_read_angle(false);
    }
    float angle_end = sum / 10.0f;//as5047->as5047_read_angle(false);
    if(abs(angle_end - angle_start) < 1){
        while(true){
            halt_pwm();
            usb_printf("error :%lf %lf\r\n",angle_start,angle_end);
            rt_thread_mdelay(5);
        }
    }
    float angle_offset = abs(angle_end - angle_start);
    if( angle_offset > 200)
        angle_offset = 360 - angle_offset;
    Pole_Pair = floor((90.f /  (float)angle_offset)) + 1;
    mechanical_offset = angle_start;

    //如果校准的极对数过大,则认为极对数校准失败
    if( Pole_Pair > 15){
        halt_pwm();
        while( true ) {
            float a = as5047->as5047_read_angle(false);
            usb_printf("Pole_Pair error : %lf  %d %lf %lf %lf  %lf %lf\r\n", mechanical_offset, Pole_Pair, angle_offset,
                       angle_start, angle_end,a);
            rt_thread_mdelay(100);
        }
    }
    set_UqUd(0.5,0.0); //校准完成后，开始转动

    //检测电机的选择方向
    float angle_pre = as5047->as5047_read_angle(false);
    for(int i=0;i < 60;i++){
        ctrl_open_Loop();
        HAL_Delay(1);
    }
    float angle_last = as5047->as5047_read_angle(false);

    if(abs(angle_pre - angle_last) > 3 )
        motor_rotate_direct = !motor_rotate_direct;
    usb_printf("Pole_Pair ok : %lf  %d %lf %lf %lf %d\r\n", mechanical_offset, Pole_Pair, angle_offset,angle_end,angle_start,motor_rotate_direct);
    Pole_Pair = 7;
    set_UqUd(0.46,0.2);
}

void Aurora_FOC::get_angle(){
    float angle_d = as5047->as5047_read_angle(true);
    angle = angle * 0.3f + 0.7f * angle_d;
    pos_gap = as5047->as5047_read_gap();
    if( angle_d < mechanical_offset)
        angle_d = mechanical_offset - angle_d;
    else
        angle_d = mechanical_offset + (360.0f - angle_d);
    if( motor_rotate_direct == ENABLE)
        mechanical_angle =  360 - angle_d;
    else
        mechanical_angle = angle_d;
    if(_wave_type == SEND_FOC_Position){
        sendWave(370.0f,-10.0f,angle,as5047->speed_rad,position,5.5f);
    }
}
/** 暂停控制 **/
void Aurora_FOC::ctrl_halt_Loop(){
    get_angle();
    speed = as5047->as5047_read_speed();

}
/** 开环控制控制 **/
void Aurora_FOC::ctrl_open_Loop(){
    get_angle();

    cal_angle_sincos();
    clark();
    park();
    ipark();   //反Park变换
    generate_svpwm();
   // usb_printf("%lf,%lf\r\n",Iq,Id);
   //usb_printf("%lf,%lf,%lf,%lf\r\n",Ia,Ib,speed,angle);
}



/** 电流控制 **/
void Aurora_FOC::ctrl_I_Loop(){
    get_angle();
    cal_angle_sincos();
    clark();
    park();
    Uq  = I_pid_q.cal_PI_AntiSaturation(Iq);
    Ud  = I_pid_d.cal_PI_AntiSaturation(Id);

    //对Uq 和  Ud进行幅度限制
    Uq = ( Uq > UqUd_Limit_Max)  ? UqUd_Limit_Max :
         ( Uq < -UqUd_Limit_Max) ? -UqUd_Limit_Max : Uq;
    Ud = ( Ud > UqUd_Limit_Max)  ? UqUd_Limit_Max :
         ( Ud < -UqUd_Limit_Max) ? -UqUd_Limit_Max : Ud;

    ipark();
    generate_svpwm();
   // usb_printf("Uq: %lf Ud: %lf  Iq : %lf   Id : %lf\r\n",Uq,Ud,Iq,I_pid_q._desired_value);
   // usb_printf("%lf,%lf\r\n",Iq,Id);
   // usb_printf("%lf,%lf,%lf\r\n",Ia,Ib,Ic);
}
void Aurora_FOC::ctrl_speed_Loop(){
    get_angle();
    cal_angle_sincos();
    clark();
    park();

    if( as5047->as5047_read_speed_update() == 1) {
        float speed_rad = as5047->as5047_read_speed();
        float desire = Speed_pid.cal_PI(speed_rad);
        I_pid_q.set_desireValue(desire); //设置期望力矩
    //    usb_printf("%lf\r\n", speed_rad);
    }

    Uq  = I_pid_q.cal_PI_AntiSaturation(Iq);
    Ud  = I_pid_d.cal_PI_AntiSaturation(Id);

    //对Uq 和  Ud进行幅度限制
    Uq = ( Uq > UqUd_Limit_Max)  ? UqUd_Limit_Max :
         ( Uq < -UqUd_Limit_Max) ? -UqUd_Limit_Max : Uq;
    Ud = ( Ud > UqUd_Limit_Max)  ? UqUd_Limit_Max :
         ( Ud < -UqUd_Limit_Max) ? -UqUd_Limit_Max : Ud;
    ipark();
    generate_svpwm();
}

/* 位置环*/
void Aurora_FOC::ctrl_position_Loop(){
    get_angle();
    cal_angle_sincos();
    clark();
    park();


    if( as5047->as5047_read_speed_update() == 1) {
        position += pos_gap;
        float position_error = Position_pid.cal_PI(position);
        Speed_pid.set_desireValue(position_error*0.01f);
        float speed_rad = as5047->as5047_read_speed();
        float desireI = Speed_pid.cal_PI(speed_rad);
        I_pid_q.set_desireValue(desireI); //设置期望力矩
    }
    Uq  = I_pid_q.cal_PI_AntiSaturation(Iq);
    Ud  = I_pid_d.cal_PI_AntiSaturation(Id);

    //对Uq 和  Ud进行幅度限制
    Uq = ( Uq > UqUd_Limit_Max)  ? UqUd_Limit_Max :
         ( Uq < -UqUd_Limit_Max) ? -UqUd_Limit_Max : Uq;
    Ud = ( Ud > UqUd_Limit_Max)  ? UqUd_Limit_Max :
         ( Ud < -UqUd_Limit_Max) ? -UqUd_Limit_Max : Ud;
    ipark();
    generate_svpwm();
}
void Aurora_FOC::set_send_wave_type(Aurora_SEND_FOC_TYPE wave_type){
    _wave_type = wave_type;
}

void Aurora_FOC::set_Wave_TaTbTc_MAXMIN(float max,float min){
    _tx_max = max;
    _tx_min = min;
}
void Aurora_FOC::set_Wave_IaIbIc_MAXMIN(float max,float min){
    _Ix_max = max;
    _Ix_min = min;
}
void Aurora_FOC::set_Wave_IqId_MAXMIN(float max,float min){
    _Iqd_max = max;
    _Iqd_min = min;
}
void Aurora_FOC::set_Wave_IalphaIbeta_MAXMIN(float max,float min){
    _Ialphabeta_max = max;
    _Ialphabeta_min = min;
}
void Aurora_FOC::Set_Iq_PI(float P,float I){
    I_pid_q.set_PID(P,I,0);
}
void Aurora_FOC::Set_Id_PI(float P,float I){
    I_pid_d.set_PID(P,I,0);
}


void Aurora_FOC::load_foc_parameter(FOC_PARAMETER_STORAGE_VALUE value){
    Pole_Pair = value.Pole_Pair;
    mechanical_offset = value.mechanical_offset.f;
    motor_rotate_direct = value.motor_rotate_direct;

    UqUd_Limit_Max = value.UqUd_Limit_Max.f;
    IqId_Limit_Max = value.IqId_Limit_Max.f;

    I_pid_q.set_PID(value.Iq_P.f,value.Iq_I.f,0);
    I_pid_d.set_PID(value.Id_P.f,value.Id_I.f,0);
    Speed_pid.set_PID(value.Speed_P.f,value.Speed_I.f,0);
    Position_pid.set_PID(value.Position_P.f,value.Position_I.f,0);

    usb_printf("%d %lf %d\r\n",Pole_Pair,mechanical_offset,motor_rotate_direct);
}
void Aurora_FOC::save_foc_parameter(uint8_t sel_M){
    if( sel_M == LEFT_MOTOR_ID) {
        foc2ui_msg.foc_param.L_FOC_PARM.storage_flag           = PARAM_VALID;
        foc2ui_msg.foc_param.L_FOC_PARM.Pole_Pair              = Pole_Pair;
        foc2ui_msg.foc_param.L_FOC_PARM.mechanical_offset.f    = mechanical_offset;
        foc2ui_msg.foc_param.L_FOC_PARM.motor_rotate_direct    = motor_rotate_direct;
        foc2ui_msg.foc_param.L_FOC_PARM.UqUd_Limit_Max.f       = UqUd_Limit_Max;
        foc2ui_msg.foc_param.L_FOC_PARM.IqId_Limit_Max.f       = IqId_Limit_Max;
        foc2ui_msg.foc_param.L_FOC_PARM.Iq_P.f                 = I_pid_q._P;
        foc2ui_msg.foc_param.L_FOC_PARM.Iq_I.f                 = I_pid_q._I;
        foc2ui_msg.foc_param.L_FOC_PARM.Id_P.f                 = I_pid_d._P;
        foc2ui_msg.foc_param.L_FOC_PARM.Id_I.f                 = I_pid_d._I;
        foc2ui_msg.foc_param.L_FOC_PARM.Speed_P.f              = Speed_pid._P;
        foc2ui_msg.foc_param.L_FOC_PARM.Speed_I.f              = Speed_pid._I;
        foc2ui_msg.foc_param.L_FOC_PARM.Position_P.f           = Position_pid._P;
        foc2ui_msg.foc_param.L_FOC_PARM.Position_I.f           = Position_pid._I;

        foc2ui_msg.type = save_focL_param;
    }
    else if( sel_M == RIGHT_MOTOR_ID) {
        foc2ui_msg.foc_param.R_FOC_PARM.storage_flag = PARAM_VALID;
        foc2ui_msg.foc_param.R_FOC_PARM.Pole_Pair = Pole_Pair;
        foc2ui_msg.foc_param.R_FOC_PARM.mechanical_offset.f = mechanical_offset;
        foc2ui_msg.foc_param.R_FOC_PARM.motor_rotate_direct = motor_rotate_direct;
        foc2ui_msg.foc_param.R_FOC_PARM.UqUd_Limit_Max.f    = UqUd_Limit_Max;
        foc2ui_msg.foc_param.R_FOC_PARM.IqId_Limit_Max.f    = IqId_Limit_Max;
        foc2ui_msg.foc_param.R_FOC_PARM.Iq_P.f = I_pid_q._P;
        foc2ui_msg.foc_param.R_FOC_PARM.Iq_I.f = I_pid_q._I;
        foc2ui_msg.foc_param.R_FOC_PARM.Id_P.f = I_pid_d._P;
        foc2ui_msg.foc_param.R_FOC_PARM.Id_I.f = I_pid_d._I;
        foc2ui_msg.foc_param.R_FOC_PARM.Speed_P.f = Speed_pid._P;
        foc2ui_msg.foc_param.R_FOC_PARM.Speed_I.f = Speed_pid._I;
        foc2ui_msg.foc_param.R_FOC_PARM.Position_P.f = Position_pid._P;
        foc2ui_msg.foc_param.R_FOC_PARM.Position_I.f = Position_pid._I;

        foc2ui_msg.type = save_focR_param;
    }
    foc2ui_msg.cmd = IS_CMD;
}
void Aurora_FOC::report_foc_parameter(uint8_t sel_M){
    if( sel_M == LEFT_MOTOR_ID){
        foc2comm_msg.foc_param.L_FOC_PARM.storage_flag           = PARAM_VALID;
        foc2comm_msg.foc_param.L_FOC_PARM.Pole_Pair              = Pole_Pair;
        foc2comm_msg.foc_param.L_FOC_PARM.mechanical_offset.f    = mechanical_offset;
        foc2comm_msg.foc_param.L_FOC_PARM.motor_rotate_direct    = motor_rotate_direct;
        foc2comm_msg.foc_param.L_FOC_PARM.UqUd_Limit_Max.f       = UqUd_Limit_Max;
        foc2comm_msg.foc_param.L_FOC_PARM.IqId_Limit_Max.f       = IqId_Limit_Max;
        foc2comm_msg.foc_param.L_FOC_PARM.Iq_P.f                 = I_pid_q._P;
        foc2comm_msg.foc_param.L_FOC_PARM.Iq_I.f                 = I_pid_q._I;
        foc2comm_msg.foc_param.L_FOC_PARM.Id_P.f                 = I_pid_d._P;
        foc2comm_msg.foc_param.L_FOC_PARM.Id_I.f                 = I_pid_d._I;
        foc2comm_msg.foc_param.L_FOC_PARM.Speed_P.f              = Speed_pid._P;
        foc2comm_msg.foc_param.L_FOC_PARM.Speed_I.f              = Speed_pid._I;
        foc2comm_msg.foc_param.L_FOC_PARM.Position_P.f           = Position_pid._P;
        foc2comm_msg.foc_param.L_FOC_PARM.Position_I.f           = Position_pid._I;
        foc2comm_msg.type = foc2comm_focL_param;
    }
    else{
        foc2comm_msg.foc_param.R_FOC_PARM.storage_flag = PARAM_VALID;
        foc2comm_msg.foc_param.R_FOC_PARM.Pole_Pair = Pole_Pair;
        foc2comm_msg.foc_param.R_FOC_PARM.mechanical_offset.f = mechanical_offset;
        foc2comm_msg.foc_param.R_FOC_PARM.motor_rotate_direct = motor_rotate_direct;
        foc2comm_msg.foc_param.R_FOC_PARM.UqUd_Limit_Max.f    = UqUd_Limit_Max;
        foc2comm_msg.foc_param.R_FOC_PARM.IqId_Limit_Max.f    = IqId_Limit_Max;
        foc2comm_msg.foc_param.R_FOC_PARM.Iq_P.f = I_pid_q._P;
        foc2comm_msg.foc_param.R_FOC_PARM.Iq_I.f = I_pid_q._I;
        foc2comm_msg.foc_param.R_FOC_PARM.Id_P.f = I_pid_d._P;
        foc2comm_msg.foc_param.R_FOC_PARM.Id_I.f = I_pid_d._I;
        foc2comm_msg.foc_param.R_FOC_PARM.Speed_P.f = Speed_pid._P;
        foc2comm_msg.foc_param.R_FOC_PARM.Speed_I.f = Speed_pid._I;
        foc2comm_msg.foc_param.R_FOC_PARM.Position_P.f = Position_pid._P;
        foc2comm_msg.foc_param.R_FOC_PARM.Position_I.f = Position_pid._I;
        foc2comm_msg.type = foc2comm_focR_param;
    }
    foc2comm_msg.cmd = IS_CMD;

}


void Aurora_FOC::sendWave(float max,float min,float data1,float data2, float data3,float data4){
    foc2ui_msg.type = update_wave;
    foc2ui_msg.wave_data.wave_value_max = max;
    foc2ui_msg.wave_data.wave_value_min = min;
    foc2ui_msg.wave_data.wave1_value    = data1;
    foc2ui_msg.wave_data.wave2_value    = data2;
    foc2ui_msg.wave_data.wave3_value    = data3;
    foc2ui_msg.cmd = IS_CMD;

    foc2comm_msg.type = foc2comm_wave_data;
    foc2comm_msg.waveData.data1 = data1;
    foc2comm_msg.waveData.data2 = data2;
    foc2comm_msg.waveData.data3 = data3;
    foc2comm_msg.waveData.data4 = data4;
    foc2comm_msg.cmd = IS_CMD;


}